#ifndef __CONTROLLER_VISUALIZATION_H__
#define __CONTROLLER_VISUALIZATION_H__

#include <ros/ros.h>
#include "common/rviz_visualization/rviz_marker.h"
#include "core/common/core.h"
#include "core/convert/msg_convert.h"
#include "core/debug/tracked_path.h"
#include "core/robot_model/robot_model_base.h"

class MCVisualization{
    public:
        MCVisualization();
        ~MCVisualization() = default;

        // 设置机器人模型
        void setRobotModel(boost::shared_ptr<ModelBase> model_ptr);

        // 发布全局路径
        void publishGolbalPath(vector<geometry_msgs::PoseStamped> poses);
        void publishGolbalPath(vector<geometry_msgs::Pose> poses);

        // 发布局部路径
        void publishLocalPath(vector<geometry_msgs::PoseStamped> poses);
        void publishLocalPath(vector<geometry_msgs::Pose> poses);

        // 添加并发布跟踪轨迹
        void publishTrackingPose(geometry_msgs::PoseStamped robot_pose);

        // 发布机器人模型
        void publishRobotModel(string frame_id);

        // 发布全局路径Marker 路径需要有方向信息提供
        void publishGolbalPathMarker(vector<geometry_msgs::PoseStamped> poses);

        // 发布预瞄点
        void publishLookaheadPose(geometry_msgs::PoseStamped lookahead_pose, string frame_id);

    private:
        ros::NodeHandle nh_;
        ros::Publisher path_global_pub_;
        ros::Publisher path_global_marker_pub_;
        ros::Publisher path_local_pub_;  
        ros::Publisher path_tracked_Pub_;
        ros::Publisher robot_model_marker_pub_;    //机器人模型发布者
        ros::Publisher lookahead_pose_pub_;

        // 实例化
        TrackedPath trackedPath_;       //路径跟踪
        boost::shared_ptr<ModelBase> robotModelPtr_;        //机器人模型
        RvizMarker rvizMarker_;

        visualization_msgs::MarkerArray last_global_path_marker_;
};

#endif
